package math;

public class Quaternion {
    public float x;
    public float y;
    public float z;
    public float w;

    public Quaternion(float x, float y, float z, float w){
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }

    public Quaternion(){
        this.x = 0;
        this.y = 0;
        this.z = 0;
        this.w = 1;
    }

    public Quaternion setFromUnitVectors(Vector3 vFrom, Vector3  vTo){
        Vector3 v1 = new Vector3();
        float r;
        float EPS = 0.000001f;

        r = vFrom.dot( vTo ) + 1;

        if ( r < EPS ) {

            r = 0;

            if ( Math.abs( vFrom.x ) > Math.abs( vFrom.z ) ) {

                v1.set( - vFrom.y, vFrom.x, 0 );

            } else {

                v1.set( 0, - vFrom.z, vFrom.y );

            }

        } else {

            v1.crossVectors( vFrom, vTo );

        }

        this.x = v1.x;
        this.y = v1.y;
        this.z = v1.z;
        this.w = r;

        return this.normalize();
    }

    public Quaternion normalize(){
        float l = this.length();

        if ( l == 0 ) {

            this.x = 0;
            this.y = 0;
            this.z = 0;
            this.w = 1;

        } else {

            l = 1 / l;

            this.x = this.x * l;
            this.y = this.y * l;
            this.z = this.z * l;
            this.w = this.w * l;

        }

        return this;
    }

    public float length(){
        return (float) Math.sqrt( this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w );
    }

    public Quaternion multiplyQuaternions( Quaternion a, Quaternion b ) {

        float qax = a.x, qay = a.y, qaz = a.z, qaw = a.w;
        float qbx = b.x, qby = b.y, qbz = b.z, qbw = b.w;

        this.x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
        this.y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
        this.z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
        this.w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;

        return this;

    }

    public Quaternion setFromEuler( Euler euler) {

        float x = euler.x, y = euler.y, z = euler.z;
        String order = euler.order;

        float c1 = (float) Math.cos( x / 2 );
        float c2 = (float) Math.cos( y / 2 );
        float c3 = (float) Math.cos( z / 2 );

        float s1 = (float) Math.sin( x / 2 );
        float s2 = (float) Math.sin( y / 2 );
        float s3 = (float) Math.sin( z / 2 );

        if (order.equals("XYZ")) {

            this.x = s1 * c2 * c3 + c1 * s2 * s3;
            this.y = c1 * s2 * c3 - s1 * c2 * s3;
            this.z = c1 * c2 * s3 + s1 * s2 * c3;
            this.w = c1 * c2 * c3 - s1 * s2 * s3;

        } else if (order.equals("YXZ")) {

            this.x = s1 * c2 * c3 + c1 * s2 * s3;
            this.y = c1 * s2 * c3 - s1 * c2 * s3;
            this.z = c1 * c2 * s3 - s1 * s2 * c3;
            this.w = c1 * c2 * c3 + s1 * s2 * s3;

        } else if (order.equals("ZXY")) {

            this.x = s1 * c2 * c3 - c1 * s2 * s3;
            this.y = c1 * s2 * c3 + s1 * c2 * s3;
            this.z = c1 * c2 * s3 + s1 * s2 * c3;
            this.w = c1 * c2 * c3 - s1 * s2 * s3;

        } else if (order.equals("ZYX")) {

            this.x = s1 * c2 * c3 - c1 * s2 * s3;
            this.y = c1 * s2 * c3 + s1 * c2 * s3;
            this.z = c1 * c2 * s3 - s1 * s2 * c3;
            this.w = c1 * c2 * c3 + s1 * s2 * s3;

        } else if (order.equals("YZX")) {

            this.x = s1 * c2 * c3 + c1 * s2 * s3;
            this.y = c1 * s2 * c3 + s1 * c2 * s3;
            this.z = c1 * c2 * s3 - s1 * s2 * c3;
            this.w = c1 * c2 * c3 - s1 * s2 * s3;

        } else if (order.equals("XZY")) {

            this.x = s1 * c2 * c3 - c1 * s2 * s3;
            this.y = c1 * s2 * c3 - s1 * c2 * s3;
            this.z = c1 * c2 * s3 + s1 * s2 * c3;
            this.w = c1 * c2 * c3 + s1 * s2 * s3;

        }

        return this;

    }
}
